#include "WPILib.h"
#include "Commands/Command.h"
#include "CommandBase.h"
#include "Robotmap.h"
#include "DigitalInput.h"
#include "Commands/AutoPos1.h"
#include "Commands/AutoPos2.h"
#include "Commands/AutoPos3.h"
#include "Commands/AutoPos4.h"
//Comment this to disable dash
#define DASH_ENABLE

class CommandBasedRobot: public IterativeRobot {
	Command *autonomousCommand;
	LiveWindow *lw;
	Compressor *comp;
	DigitalInput *auto1;
	DigitalInput *auto2;
	bool autoRun;

public:
	//	static Timer gameTimer;
	virtual void RobotInit() {
		CommandBase::init();
		comp = new Compressor(COMPRESSOR_PRESSURE_SWITCH, COMPRESSOR_SPIKE);
		DriverStationLCD::GetInstance()->PrintfLine(
				DriverStationLCD::kUser_Line1, "Rev: Comp1");

		CommandBase::discHandling->pinTimer.Start();
		CommandBase::breach->Init();

		auto1 = new DigitalInput(AUTO_SWITCH_1);
		auto2 = new DigitalInput(AUTO_SWITCH_2);

		autoRun = false;
		//		lw = LiveWindow::GetInstance();
		//		lw->SetEnabled(1);
		//		CommandBasae::drivetrain->leftEncoder.StartLiveWindowMode();
		//		CommandBase::drivetrain->rightEncoder.StartLiveWindowMode();
		//		lw->AddComponent("DriveTrain","Left Encoder", &CommandBase::drivetrain->leftEncoder);
		//		lw->AddComponent("DriveTrain","Right Encoder", &CommandBase::drivetrain->rightEncoder);
		//		lw->Run();
		//		lw->AddComponent("A","L E",&CommandBase::drivetrain->leftEncoder);
#ifdef DASH_ENABLE
		printf("Connecting to dash");
		CommandBase::client->ConnectToDash();
		printf("Finished connect attempt");
#endif
	}

	virtual void AutonomousInit() {
		CommandBase::discHandling->AUTO = true;
		int a1 = auto1->Get();
		int a2 = auto2->Get();
		int pos = a1 + (a2 * 2) + 1; // Turns switches into 1-4
		autoRun = true;
		//		switch (pos) {
		//		case 1: // Both off
		//			autonomousCommand = new AutoPos1();
		//			break;
		//		case 2: // 1 on, 2 off
		//			autonomousCommand = new AutoPos2();
		//			break;
		//		case 3: // 1 off, 2 on
		//			autonomousCommand = new AutoPos3();
		//			break;
		//		case 4: // Both on 
		//			autonomousCommand = new AutoPos4();
		//			break;
		//		default:
		//			autonomousCommand = new AutoPos1();
		//		}
		autonomousCommand = new AutoPos1();
		autonomousCommand->Start();
		CommandBase::shooterAngle->Enable();
		//		gameTimer.Start();
		comp->Start();

		CommandBase::climbing->KeepPopperDown();
		CommandBase::climbing->thePuller.Set(false);
		CommandBase::client->ConnectToDash();

	}

	virtual void AutonomousPeriodic() {
		Scheduler::GetInstance()->Run();
		//		CommandBase::shooter->PeriodicUpdate();
	}

	virtual void TeleopInit() {
		CommandBase::discHandling->AUTO = false;
		//		autonomousCommand->Cancel();
		comp->Start();
		//		CommandBase::client->ConnectToDash()
		CommandBase::climbing->thePuller.Set(false);
		CommandBase::climbing->KeepPopperDown();
		CommandBase::discHandling->startConveyor->StartSingle(1);
		if (autoRun) {
			autonomousCommand->Cancel();
			CommandBase::climbing->pullerNotifier->StartSingle(119.0);
			CommandBase::climbing->popperNotifier->StartSingle(110.0);
		}
		// This makes sure that the autonomous stops running when
		// teleop starts running. If you want the autonomous to 
		// continue until interrupted by another command, remove
		// this line or comment it out.

		CommandBase::shooterAngle->Enable();
	}

	virtual void TeleopPeriodic() {
		Scheduler::GetInstance()->Run();
		DriverStationLCD::GetInstance()->UpdateLCD();
		CommandBase::client->PeriodicUpdate();
	}
	virtual void DisabledInit() {
		CommandBase::breach->ControlBreachState(
				CommandBase::breach->kBreachDown);
		CommandBase::discHandling->ControlConveyorState(
				CommandBase::discHandling->kConveyorOff);
		CommandBase::discHandling->ControlPinState(false);
		CommandBase::drivetrain->DriveRobot(0.0, 0.0);
		comp->Stop();
		CommandBase::discMagnet->discMagnetVictor.Set(Relay::kOff);
	}
	virtual void DisabledPeriodic() {
		CommandBase::discMagnet->discMagnetVictor.Set(Relay::kOff);
		CommandBase::breach->ControlBreachState(
				CommandBase::breach->kBreachDown);
		CommandBase::discHandling->ControlConveyorState(
				CommandBase::discHandling->kConveyorOff);
		CommandBase::discHandling->ControlPinState(false);
		CommandBase::drivetrain->DriveRobot(0.0, 0.0);
		comp->Stop();
	}
};
START_ROBOT_CLASS(CommandBasedRobot)
;
